#include "dispatcher_service.h"

#include <QHash>
#include <QString>
#include <QVector>
#include <algorithm>
#include <cmath>
#include <limits>
#include <queue>
#include <set>
#include <unordered_map>

#include "application/dispatcher/dispatcher_manager.h"
#include "application/map/lift/lift_manager.h"`n#include "application/map/park/park_manager.h"`n#include "application/map/charge/charge_manager.h"`n#include "application/map/third_area/third_area_manager.h"

namespace application {
namespace dispatcher {

DispatcherService::DispatcherService(
    std::shared_ptr<vehicle::VehicleService> vehicleService,
    std::shared_ptr<MapApplicationService> mapService,
    std::shared_ptr<infrastructure::services::CommonService> commonService)
    : m_vehicleService(vehicleService),
      m_mapService(mapService),
      m_logger(commonService ? commonService->getLogger("DispatcherService")
                             : nullptr) {
  if (commonService) {
    m_config = commonService->configuration();
  }

  // 创建并初始化多地图调度管理器
  m_dispatcherManager = std::make_shared<DispatcherManager>(
      m_vehicleService, m_mapService, m_logger);
  m_dispatcherManager->init();

  if (m_logger) {
    m_logger->info("DispatcherService initialized with multi-map dispatcher");
  }
}

// ==================== AGV 调度 ====================

/**
 * @brief 请求AGV导航到目标站点（只记录目标，不立即下发）
 * @details 参考三方代码 dispatcher.cpp:432-460 AgvNavToStation
 */
bool DispatcherService::requestNavigation(kernel::agv::AgvEntity* agv,
                                          const std::string& targetStation,
                                          int floor,
                                          const std::string& operation) {
  if (!agv) {
    if (m_logger) {
      m_logger->error("requestNavigation: AGV pointer is null");
    }
    return false;
  }

  // 检查AGV是否已经在目标站点（参考三方代码 dispatcher.cpp:445-455）
  std::string currentStation = agv->getCurrentStation();

 if (currentStation == targetStation) {
    if (m_logger) {
      m_logger->info(
          QString("AGV %1 already at target station %2, navigation not needed")
              .arg(agv->getNumber())
              .arg(QString::fromStdString(targetStation)));
    }
    // 取消调度器控制（参考三方代码 dispatcher.cpp:453）
    agv->setControlledByDispatcher(false);
    return true;
  }

  // 设置AGV被调度器控制（参考三方代码 dispatcher.cpp:438）
  agv->setControlledByDispatcher(true);

  if (m_logger) {
    m_logger->info(QString("Requesting navigation: AGV%1 -> %2 (floor=%3)")
                       .arg(agv->getNumber())
                       .arg(QString::fromStdString(targetStation))
                       .arg(floor));
  }

  // 委托给 DispatcherManager 处理（它会找到对应楼层的 RouteDispatcher）
  if (!m_dispatcherManager) {
    if (m_logger) {
      m_logger->error("DispatcherManager not available");
    }
    agv->setControlledByDispatcher(false);
    return false;
  }

  // DispatcherManager 会将请求提交给对应楼层的 RouteDispatcher
  // RouteDispatcher 会进行多智能体路径规划并按需下发任务
  auto handle = m_dispatcherManager->stationTaskFromServer(
      agv->getNumber(),
      QString::fromStdString(targetStation),
      floor,
      QString::fromStdString(operation));

  if (handle.moveStatus != MoveFunctionStatus::MoveOk) {
    QString errorReason;
    switch (handle.moveStatus) {
      case MoveFunctionStatus::MoveError:
        errorReason = "AGV not found or internal error";
        break;
      case MoveFunctionStatus::MoveCharging:
        errorReason = "AGV is charging or in charge area";
        // 充电状态可能需要等待，不一定是错误
        if (m_logger) {
          m_logger->warning(QString("AGV%1 is charging, navigation pending")
                               .arg(agv->getNumber()));
        }
        return true;  // 返回成功，等待充电完成
      case MoveFunctionStatus::MoveNeedPassFloor:
        errorReason = "AGV is on different floor (cross-floor not supported)";
        break;
      case MoveFunctionStatus::MoveNoIdle:
        errorReason = "No idle AGV available";
        break;
      default:
        errorReason = "Unknown error";
        break;
    }

    if (m_logger) {
      m_logger->error(QString("Failed to submit navigation request for AGV%1: %2")
                          .arg(agv->getNumber())
                          .arg(errorReason));
    }
    agv->setControlledByDispatcher(false);
    return false;
  }

  // TODO: 设置 finalOperation（MKLM AGV）
  // if (!operation.empty() && agv->getType() == MKLM) {
  //     agv->setFinalOperation(targetStation, operation);
  // }

  return true;
}

void DispatcherService::cancelPendingForAgv(int agvNumber) {
  if (!m_dispatcherManager) {
    return;
  }
  m_dispatcherManager->cancelPendingForAgv(agvNumber);
}

bool DispatcherService::rescheduleAgvTask(int agvNumber) {
  if (!m_vehicleService || !m_dispatcherManager) {
    if (m_logger) {
      m_logger->warning("DispatcherService unavailable for reschedule");
    }
    return false;
  }

  auto* agv = m_vehicleService->getVehicle(agvNumber);
  if (!agv || !agv->isOnline()) {
    if (m_logger) {
      m_logger->warning(QString("Reschedule failed: AGV %1 not found or offline").arg(agvNumber));
    }
    return false;
  }

  int floor = resolveAgvFloor(agv);
  if (floor == -1) {
    if (m_logger) {
      m_logger->warning(QString("Reschedule failed: cannot resolve floor for AGV %1").arg(agvNumber));
    }
    return false;
  }

  auto* dispatcher = m_dispatcherManager->getDispatcher(floor);
  if (!dispatcher) {
    if (m_logger) {
      m_logger->warning(QString("Reschedule failed: no dispatcher for floor %1 (AGV %2)").arg(floor).arg(agvNumber));
    }
    return false;
  }

  return dispatcher->rescheduleAgvTask(agvNumber);
}

void DispatcherService::setDispatchCallback(
    std::function<void(int, const QList<QString>&)> cb) {
  if (m_dispatcherManager) {
    m_dispatcherManager->setDispatchCallback(std::move(cb));
  }
}

void DispatcherService::setSolveFailureCallback(
    std::function<void(const QString&)> cb) {
  if (m_dispatcherManager) {
    m_dispatcherManager->setSolveFailureCallback(std::move(cb));
  }
}

// ==================== 区域管理 ====================

bool DispatcherService::checkAreaConflict(const std::string& station,
                                          int agvNumber, int floor) {
  if (!m_mapService) return false;
  auto areaMgr = m_mapService->getAreaManager(floor);
  if (!areaMgr) return false;
  const QString areaId = QString::fromStdString(station);
  auto area = areaMgr->getArea(areaId);
  if (!area) return false;
  if (agvNumber < 0) {
    return area->isOccupied();
  }
  return area->isOccupied() && !area->isOccupiedBy(agvNumber);
}

bool DispatcherService::occupyArea(const std::string& station, int agvNumber,
                                   int floor) {
  if (!m_mapService) return true;
  auto areaMgr = m_mapService->getAreaManager(floor);
  if (!areaMgr) return true;
  const QString areaId = QString::fromStdString(station);
  if (!areaMgr->hasArea(areaId)) return true;

  // 普通区域占用检查：若已被其他AGV占用则拒绝
  auto area = areaMgr->getArea(areaId);
  if (area && area->isOccupied() && !area->isOccupiedBy(agvNumber)) {
    if (m_logger) {
      m_logger->debug(QString("Area busy: %1 occupied by another AGV").arg(areaId));
    }
    return false;
  }

  if (!tryAcquireThirdPartyArea(areaId, agvNumber, floor)) {
    if (m_logger) {
      m_logger->debug(QString("Third-party area busy: %1").arg(areaId));
    }
    return false;
  }

  areaMgr->enterArea(areaId, agvNumber);
  return true;
}

void DispatcherService::releaseArea(const std::string& station, int agvNumber,
                                    int floor) {
  const QString areaId = QString::fromStdString(station);
  releaseThirdPartyArea(areaId, agvNumber, floor);

  if (!m_mapService) return;
  auto areaMgr = m_mapService->getAreaManager(floor);
  if (!areaMgr) return;
  if (!areaMgr->hasArea(areaId)) return;
  areaMgr->exitArea(areaId, agvNumber);
}

void DispatcherService::releaseAllAreasForAgv(int agvNumber) {
  // TODO: 释放AGV占用的所有区�?

  if (m_logger) {
    m_logger->debug(QString("Released all areas for AGV %1").arg(agvNumber));
  }
}

// ==================== 状态查�?====================

std::vector<int> DispatcherService::getIdleAgvs(int floor) {
  std::vector<int> idleAgvs;

  auto vehicles = m_vehicleService->getIdleVehicles();
  for (auto* agv : vehicles) {
    // 如果指定了楼层，检查AGV是否在该楼层
    if (floor != -1 && !isAgvOnFloor(agv->getNumber(), floor)) {
      continue;
    }

    idleAgvs.push_back(agv->getNumber());
  }

  return idleAgvs;
}

bool DispatcherService::isAgvOnFloor(int agvNumber, int floor) {
  auto* agv = m_vehicleService->getVehicle(agvNumber);
  if (!agv) {
    return false;
  }

  std::string mapName = getMapNameForFloor(floor);
  return agv->getCurrentMap() == mapName;
}

bool DispatcherService::isAgvCharging(int agvNumber) {
  auto* agv = m_vehicleService->getVehicle(agvNumber);
  if (!agv) {
    return false;
  }
  return agv->isCharging();
}

std::shared_ptr<park::ParkPoint> DispatcherService::allocateParkingPoint(
    kernel::agv::AgvEntity* agv,
    int preferredFloor) {
  if (!m_mapService || !agv) {
    return nullptr;
  }

  int currentFloor = preferredFloor;
  if (currentFloor <= 0) {
    currentFloor = resolveAgvFloor(agv);
  }

  auto loadedFloors = m_mapService->getLoadedFloors();
  if (loadedFloors.isEmpty()) {
    return nullptr;
  }

  std::vector<int> uniqueFloors;
  uniqueFloors.reserve(loadedFloors.size());
  for (int floor : loadedFloors) {
    if (floor <= 0) {
      continue;
    }
    if (std::find(uniqueFloors.begin(), uniqueFloors.end(), floor) ==
        uniqueFloors.end()) {
      uniqueFloors.push_back(floor);
    }
  }

  if (currentFloor > 0 &&
      std::find(uniqueFloors.begin(), uniqueFloors.end(), currentFloor) ==
          uniqueFloors.end()) {
    uniqueFloors.push_back(currentFloor);
  }

  std::vector<int> searchOrder;
  if (currentFloor > 0) {
    struct FloorPriority {
      int floor;
      int hopCost;
      int floorDiff;
    };
    std::vector<FloorPriority> priorities;
    priorities.reserve(uniqueFloors.size());

    for (int floor : uniqueFloors) {
      if (floor <= 0) {
        continue;
      }
      FloorPriority entry{floor, 0, std::abs(floor - currentFloor)};
      if (floor == currentFloor) {
        entry.hopCost = 0;
      } else {
        entry.hopCost = calculateLiftHopCost(currentFloor, floor);
      }
      priorities.push_back(entry);
    }

    std::sort(priorities.begin(), priorities.end(),
              [](const FloorPriority& lhs, const FloorPriority& rhs) {
          if (lhs.hopCost != rhs.hopCost) {
            return lhs.hopCost < rhs.hopCost;
          }
          if (lhs.floorDiff != rhs.floorDiff) {
            return lhs.floorDiff < rhs.floorDiff;
          }
          return lhs.floor < rhs.floor;
        });

    for (const auto& entry : priorities) {
      searchOrder.push_back(entry.floor);
    }
  } else {
    std::sort(uniqueFloors.begin(), uniqueFloors.end());
    searchOrder = uniqueFloors;
  }

  auto pickNearestPoint =
      [&](const QList<std::shared_ptr<park::ParkPoint>>& candidates)
      -> std::shared_ptr<park::ParkPoint> {
    if (candidates.isEmpty()) {
      return nullptr;
    }
    const auto agvPos = agv->getPosition();
    double bestDistance = std::numeric_limits<double>::max();
    std::shared_ptr<park::ParkPoint> bestPoint;
    for (const auto& candidate : candidates) {
      if (!candidate) {
        continue;
      }
      const auto pos = candidate->getPosition();
      const double dx = pos.x - agvPos.x;
      const double dy = pos.y - agvPos.y;
      const double distSquared = dx * dx + dy * dy;
      if (distSquared < bestDistance) {
        bestDistance = distSquared;
        bestPoint = candidate;
      }
    }
    return bestPoint ? bestPoint : candidates.first();
  };

  for (int floor : searchOrder) {
    if (floor <= 0) {
      continue;
    }
    auto manager = m_mapService->getParkManager(floor);
    if (!manager) {
      continue;
    }
    auto candidates = manager->getAvailableParkPoints();
    auto parkPoint = pickNearestPoint(candidates);
    if (parkPoint) {
      return parkPoint;
    }
  }

  return nullptr;
}

std::shared_ptr<charge::ChargePoint> DispatcherService::allocateChargePoint(
    kernel::agv::AgvEntity* agv,
    int preferredFloor) {
  if (!m_mapService || !agv) {
    return nullptr;
  }

  int currentFloor = preferredFloor;
  if (currentFloor <= 0) {
    currentFloor = resolveAgvFloor(agv);
  }

  auto loadedFloors = m_mapService->getLoadedFloors();
  if (loadedFloors.isEmpty()) {
    return nullptr;
  }

  std::vector<int> uniqueFloors;
  uniqueFloors.reserve(loadedFloors.size());
  for (int floor : loadedFloors) {
    if (floor <= 0) {
      continue;
    }
    if (std::find(uniqueFloors.begin(), uniqueFloors.end(), floor) ==
        uniqueFloors.end()) {
      uniqueFloors.push_back(floor);
    }
  }

  if (currentFloor > 0 &&
      std::find(uniqueFloors.begin(), uniqueFloors.end(), currentFloor) ==
          uniqueFloors.end()) {
    uniqueFloors.push_back(currentFloor);
  }

  std::vector<int> searchOrder;
  if (currentFloor > 0) {
    struct FloorPriority {
      int floor;
      int hopCost;
      int floorDiff;
    };
    std::vector<FloorPriority> priorities;
    priorities.reserve(uniqueFloors.size());

    for (int floor : uniqueFloors) {
      if (floor <= 0) {
        continue;
      }
      FloorPriority entry{floor, 0, std::abs(floor - currentFloor)};
      if (floor == currentFloor) {
        entry.hopCost = 0;
      } else {
        entry.hopCost = calculateLiftHopCost(currentFloor, floor);
      }
      priorities.push_back(entry);
    }

    std::sort(priorities.begin(), priorities.end(),
              [](const FloorPriority& lhs, const FloorPriority& rhs) {
                if (lhs.hopCost != rhs.hopCost) {
                  return lhs.hopCost < rhs.hopCost;
                }
                if (lhs.floorDiff != rhs.floorDiff) {
                  return lhs.floorDiff < rhs.floorDiff;
                }
                return lhs.floor < rhs.floor;
              });

    for (const auto& entry : priorities) {
      searchOrder.push_back(entry.floor);
    }
  } else {
    std::sort(uniqueFloors.begin(), uniqueFloors.end());
    searchOrder = uniqueFloors;
  }

  auto pickNearestPoint =
      [&](const QList<std::shared_ptr<charge::ChargePoint>>& candidates)
      -> std::shared_ptr<charge::ChargePoint> {
    if (candidates.isEmpty()) {
      return nullptr;
    }
    const auto agvPos = agv->getPosition();
    double bestDistance = std::numeric_limits<double>::max();
    std::shared_ptr<charge::ChargePoint> bestPoint;
    for (const auto& candidate : candidates) {
      if (!candidate) {
        continue;
      }
      if (!candidate->supportsAgvType(agv->getType())) {
        continue;
      }
      if (!candidate->isVehicleAllowed(agv->getNumber())) {
        continue;
      }
      const auto pos = candidate->getPosition();
      const double dx = pos.x - agvPos.x;
      const double dy = pos.y - agvPos.y;
      const double distSquared = dx * dx + dy * dy;
      if (distSquared < bestDistance) {
        bestDistance = distSquared;
        bestPoint = candidate;
      }
    }
    return bestPoint ? bestPoint : candidates.first();
  };

  for (int floor : searchOrder) {
    if (floor <= 0) {
      continue;
    }
    auto manager = m_mapService->getChargeManager(floor);
    if (!manager) {
      continue;
    }
    auto candidates = manager->getAvailableChargePoints(agv->getNumber());
    auto chargePoint = pickNearestPoint(candidates);
    if (chargePoint) {
      return chargePoint;
    }
  }

  return nullptr;
}

bool DispatcherService::tryAcquireThirdPartyArea(const QString& areaId,
                                                 int agvNumber,
                                                 int floor) {
  if (!m_mapService) {
    return true;
  }

  auto thirdMgr = m_mapService->getThirdAreaManager(floor);
  if (!thirdMgr) {
    return true;
  }

  auto* thirdArea = thirdMgr->getArea(areaId);
  if (!thirdArea) {
    return true;
  }

  if (thirdArea->tryAgvAcquire(agvNumber)) {
    return true;
  }

  if (m_logger) {
    m_logger->warning(QString("Failed to occupy third-party area %1 for AGV%2")
                          .arg(areaId)
                          .arg(agvNumber));
  }
  return false;
}

void DispatcherService::releaseThirdPartyArea(const QString& areaId,
                                              int agvNumber,
                                              int floor) {
  if (!m_mapService) {
    return;
  }

  auto thirdMgr = m_mapService->getThirdAreaManager(floor);
  if (!thirdMgr) {
    return;
  }

  auto* thirdArea = thirdMgr->getArea(areaId);
  if (!thirdArea) {
    return;
  }

  thirdArea->cancelOccupy(QString::number(agvNumber));
  thirdArea->cancelOccupy(agvNumber);
}

// ==================== 私有辅助方法 ====================

int DispatcherService::selectBestIdleAgv(const std::string& targetStation,
                                         int floor,
                                         const std::vector<kernel::agv::AgvType>& allowedTypes) {
  if (!m_vehicleService) {
    return -1;
  }

  auto idleVehicles = m_vehicleService->getIdleVehicles();
  auto allVehicles = m_vehicleService->getAllVehicles();

  // 定义车型过滤函数
  auto isAgvTypeAllowed = [&allowedTypes](kernel::agv::AgvEntity* agv) -> bool {
    if (allowedTypes.empty()) {
      return true;  // 空列表表示不限制车型
    }
    auto agvType = agv->getType();
    for (const auto& allowedType : allowedTypes) {
      if (agvType == allowedType) {
        return true;
      }
    }
    return false;
  };

  int bestSameFloorAgv = -1;
  double minDistance = std::numeric_limits<double>::max();
  int maxBattery = -1;

  for (auto* agv : idleVehicles) {
    if (!agv) {
      continue;
    }
    if (resolveAgvFloor(agv) != floor) {
      continue;
    }
    // 车型过滤
    if (!isAgvTypeAllowed(agv)) {
      continue;
    }

    double distance = calculateDistance(agv, targetStation, floor);
    int battery = agv->getBatteryLevel();

    if (distance < minDistance ||
        (distance == minDistance && battery > maxBattery)) {
      minDistance = distance;
      maxBattery = battery;
      bestSameFloorAgv = agv->getNumber();
    }
  }

  if (bestSameFloorAgv != -1) {
    return bestSameFloorAgv;
  }

  // 1.1 同楼层无空闲车时，选用电量>=80的车辆（含充电中）
  auto selectSameFloorCharged = [&](int minBatteryPercent) -> int {
    int bestAgvNumber = -1;
    double bestDistance = std::numeric_limits<double>::max();
    int bestBattery = -1;
    for (auto* agv : allVehicles) {
      if (!agv) {
        continue;
      }
      if (!agv->isOnline() || agv->isControlledByDispatcher() || agv->isLocked()) {
        continue;
      }
      if (!isAgvTypeAllowed(agv)) {
        continue;
      }
      int agvFloor = resolveAgvFloor(agv);
      if (agvFloor != floor || agvFloor == -1) {
        continue;
      }
      if (agv->getBatteryLevel() < minBatteryPercent) {
        continue;
      }
      // 避免抢占执行中的车，仅在未忙碌或仅在充电状态时使用
      if (agv->isBusy() && !agv->isCharging()) {
        continue;
      }

      double distance = calculateDistance(agv, targetStation, floor);
      int battery = agv->getBatteryLevel();
      if (distance < bestDistance ||
          (distance == bestDistance && battery > bestBattery)) {
        bestDistance = distance;
        bestBattery = battery;
        bestAgvNumber = agv->getNumber();
      }
    }
    return bestAgvNumber;
  };

  int chargedSameFloorAgv = selectSameFloorCharged(80);
  if (chargedSameFloorAgv != -1) {
    return chargedSameFloorAgv;
  }

  auto liftManager = m_mapService ? m_mapService->getLiftManager() : nullptr;
  if (!liftManager) {
    if (m_logger) {
      m_logger->warning(
          "selectBestIdleAgv: cross-floor selection unavailable (no lift manager)");
    }
    return -1;
  }

  std::unordered_map<int, std::vector<kernel::agv::AgvEntity*>> floorBuckets;
  std::set<int> distinctFloors;
  for (auto* agv : idleVehicles) {
    if (!agv) {
      continue;
    }
    int agvFloor = resolveAgvFloor(agv);
    if (agvFloor == -1) {
      continue;
    }
    if (agvFloor == floor) {
      continue;  // 已经验证过同楼层
    }
    // 车型过滤
    if (!isAgvTypeAllowed(agv)) {
      continue;
    }
    floorBuckets[agvFloor].push_back(agv);
    distinctFloors.insert(agvFloor);
  }

  struct CrossFloorScore {
    int hopCount{std::numeric_limits<int>::max()};
    double distance{std::numeric_limits<double>::max()};
    int battery{-1};
  };

  auto betterScore = [](const CrossFloorScore& lhs,
                        const CrossFloorScore& rhs) -> bool {
    if (lhs.hopCount != rhs.hopCount) {
      return lhs.hopCount < rhs.hopCount;
    }
    if (lhs.distance != rhs.distance) {
      return lhs.distance < rhs.distance;
    }
    return lhs.battery > rhs.battery;
  };

  int bestAgv = -1;
  CrossFloorScore bestScore;

  auto chooseEntryStation = [](const ::application::lift::FloorInfo& info) {
    if (!info.waitPoint.isEmpty()) return info.waitPoint;
    if (!info.askPoint.isEmpty()) return info.askPoint;
    if (!info.interPoint.isEmpty()) return info.interPoint;
    return QString();
  };
  if (!floorBuckets.empty()) {
    std::vector<int> downwardFloors;
    std::vector<int> upwardFloors;
    for (int f : distinctFloors) {
      if (f <= floor) {
        downwardFloors.push_back(f);
      } else {
        upwardFloors.push_back(f);
      }
    }
    std::sort(downwardFloors.begin(), downwardFloors.end(),
              std::greater<int>());
    std::sort(upwardFloors.begin(), upwardFloors.end());

    std::vector<kernel::agv::AgvEntity*> orderedCandidates;
    orderedCandidates.reserve(idleVehicles.size());
    for (int f : downwardFloors) {
      if (f == floor) {
        continue;
      }
      auto it = floorBuckets.find(f);
      if (it == floorBuckets.end()) {
        continue;
      }
      orderedCandidates.insert(orderedCandidates.end(), it->second.begin(),
                               it->second.end());
    }
    for (int f : upwardFloors) {
      auto it = floorBuckets.find(f);
      if (it == floorBuckets.end()) {
        continue;
      }
      orderedCandidates.insert(orderedCandidates.end(), it->second.begin(),
                               it->second.end());
    }

    if (!orderedCandidates.empty()) {
      for (auto* agv : orderedCandidates) {
        if (!agv) {
          continue;
        }

        int agvFloor = resolveAgvFloor(agv);
        if (agvFloor == -1 || agvFloor == floor) {
          continue;
        }

        auto liftPath = liftManager->findLiftPath(agvFloor, floor);
        if (liftPath.isEmpty()) {
          continue;
        }

        CrossFloorScore candidateScore;
        candidateScore.hopCount = liftPath.size();
        candidateScore.battery = agv->getBatteryLevel();

        const auto entryStation =
            chooseEntryStation(liftPath.front().startFloor);
        if (!entryStation.isEmpty()) {
          candidateScore.distance =
              calculateDistance(agv, entryStation.toStdString(), agvFloor);
        }

        if (bestAgv == -1 || betterScore(candidateScore, bestScore)) {
          bestAgv = agv->getNumber();
          bestScore = candidateScore;
        }
      }
    }
  }

  if (bestAgv != -1) {
    return bestAgv;
  }

  // 3. 跨楼层无空闲车，再选电量>=40的跨楼层车（含充电中）
  std::unordered_map<int, std::vector<kernel::agv::AgvEntity*>> chargedFloorBuckets;
  std::set<int> chargedDistinctFloors;
  for (auto* agv : allVehicles) {
    if (!agv) {
      continue;
    }
    if (!agv->isOnline() || agv->isControlledByDispatcher() || agv->isLocked()) {
      continue;
    }
    if (!isAgvTypeAllowed(agv)) {
      continue;
    }
    int agvFloor = resolveAgvFloor(agv);
    if (agvFloor == -1 || agvFloor == floor) {
      continue;
    }
    if (agv->getBatteryLevel() < 40) {
      continue;
    }
    // 避免抢占非充电的执行中任务
    if (agv->isBusy() && !agv->isCharging()) {
      continue;
    }

    chargedFloorBuckets[agvFloor].push_back(agv);
    chargedDistinctFloors.insert(agvFloor);
  }

  if (chargedFloorBuckets.empty()) {
    return -1;
  }

  std::vector<int> chargedDownwardFloors;
  std::vector<int> chargedUpwardFloors;
  for (int f : chargedDistinctFloors) {
    if (f <= floor) {
      chargedDownwardFloors.push_back(f);
    } else {
      chargedUpwardFloors.push_back(f);
    }
  }
  std::sort(chargedDownwardFloors.begin(), chargedDownwardFloors.end(),
            std::greater<int>());
  std::sort(chargedUpwardFloors.begin(), chargedUpwardFloors.end());

  std::vector<kernel::agv::AgvEntity*> chargedOrderedCandidates;
  chargedOrderedCandidates.reserve(allVehicles.size());
  for (int f : chargedDownwardFloors) {
    if (f == floor) {
      continue;
    }
    auto it = chargedFloorBuckets.find(f);
    if (it == chargedFloorBuckets.end()) {
      continue;
    }
    chargedOrderedCandidates.insert(chargedOrderedCandidates.end(),
                                    it->second.begin(), it->second.end());
  }
  for (int f : chargedUpwardFloors) {
    auto it = chargedFloorBuckets.find(f);
    if (it == chargedFloorBuckets.end()) {
      continue;
    }
    chargedOrderedCandidates.insert(chargedOrderedCandidates.end(),
                                    it->second.begin(), it->second.end());
  }

  int chargedBestAgv = -1;
  CrossFloorScore chargedBestScore;
  for (auto* agv : chargedOrderedCandidates) {
    if (!agv) {
      continue;
    }

    int agvFloor = resolveAgvFloor(agv);
    if (agvFloor == -1 || agvFloor == floor) {
      continue;
    }

    auto liftPath = liftManager->findLiftPath(agvFloor, floor);
    if (liftPath.isEmpty()) {
      continue;
    }

    CrossFloorScore candidateScore;
    candidateScore.hopCount = liftPath.size();
    candidateScore.battery = agv->getBatteryLevel();

    const auto entryStation = chooseEntryStation(liftPath.front().startFloor);
    if (!entryStation.isEmpty()) {
      candidateScore.distance =
          calculateDistance(agv, entryStation.toStdString(), agvFloor);
    }

    if (chargedBestAgv == -1 || betterScore(candidateScore, chargedBestScore)) {
      chargedBestAgv = agv->getNumber();
      chargedBestScore = candidateScore;
    }
  }

  return chargedBestAgv;
}

std::string DispatcherService::getMapNameForFloor(int floor) {
  // TODO: �?MapApplicationService 获取楼层对应的地图名�?
  // 暂时简化实�?
  if (m_mapService) {
    auto name = m_mapService->getMapNameForFloor(floor);
    if (!name.isEmpty()) return name.toStdString();
  }
  return "Floor" + std::to_string(floor);
}

bool DispatcherService::isCrossFloorTask(int agvNumber, int targetFloor) {
  auto* agv = m_vehicleService->getVehicle(agvNumber);
  if (!agv) {
    return false;
  }

  std::string currentMap = agv->getCurrentMap();
  std::string targetMap = getMapNameForFloor(targetFloor);

  return currentMap != targetMap;
}

double DispatcherService::calculateDistance(kernel::agv::AgvEntity* agv,
                                            const std::string& targetStation,
                                            int floor) {
  // Try Euclidean distance using station position if available
  do {
    if (!m_mapService) break;
    if (agv->getCurrentStation() == targetStation) return 0.0;
    auto pos =
        m_mapService->getStationPosition(QString::fromStdString(targetStation),
                                         floor);
    auto aPos = agv->getPosition();
    double dx = aPos.x - pos.x;
    double dy = aPos.y - pos.y;
    return std::sqrt(dx * dx + dy * dy);
  } while (false);
  // TODO: 实现真实的距离计
  // 暂时返回一个简化的距离

  // 简化：如果AGV当前站点就是目标站点，距离为0
  if (agv->getCurrentStation() == targetStation) {
    return 0.0;
  }

  // 否则使用曼哈顿距离（需要获取站点坐标）
  // 暂时返回固定�?
  return 100.0;
}

int DispatcherService::resolveAgvFloor(const kernel::agv::AgvEntity* agv) const {
  if (!agv || !m_mapService) {
    return -1;
  }
  return m_mapService->getFloorForMapName(
      QString::fromStdString(agv->getCurrentMap()));
}

int DispatcherService::calculateLiftHopCost(int fromFloor,
                                            int targetFloor) const {
  if (fromFloor <= 0 || targetFloor <= 0) {
    return fromFloor == targetFloor ? 0 : std::numeric_limits<int>::max();
  }

  if (fromFloor == targetFloor) {
    return 0;
  }

  if (!m_mapService) {
    return std::numeric_limits<int>::max();
  }

  auto liftManager = m_mapService->getLiftManager();
  if (!liftManager) {
    return std::numeric_limits<int>::max();
  }

  auto path = liftManager->findLiftPath(fromFloor, targetFloor);
  if (path.isEmpty()) {
    return std::numeric_limits<int>::max();
  }

  return path.size();
}

}  // namespace dispatcher
}  // namespace application
